/** \file ProcessManager.cc
 * \author Robotnik Automation S.L.L.
 * \version 1.1
 * \date    2019
 *
 * \brief ProcessManager Class
 * 	Clase desarrollada para la gestión de procesos de robotrans, pudiendose lanzar, parar, reiniciar los procesos de
 *  forma remota.
 * (C) 2010 Robotnik Automation, SLL
*/
#include <ros/ros.h>
#include "ProcessManager.h"

/*!	\fn int ProcessManager::ExecTurtleSim()
 * 	\brief Creates a new process and exec player server with kanban driver
 * 	\return PROCESS_OK
 * 	\return PROCESS_ERROR_FORK
 *	\return PROCESS_ERROR_EXEC
 *	\return PROCESS_ERROR_WAITING
*/
	int ProcessManager::ExecSkype(){
        int status;
        pid_t waitreturnpid;

        if(pid_skype > 0){	//Check if it's already running
            waitreturnpid = waitpid(pid_skype, &status, WNOHANG);
            if(waitreturnpid == 0){
                printf("ProcessManager::ExecSkype: process already running\n");
                return PROCESS_RUNNING;	//process already running
            }if(waitreturnpid < 0){
                printf("ProcessManager::ExecSkype: Error in waitpid: %s\n", strerror(errno));
                pid_skype = -1;
                return PROCESS_ERROR_WAITING;
            }
        }

        if((pid_skype = fork()) == -1){
            printf("ProcessManager::ExecSkype: Error in fork: %s\n", strerror(errno));
            return PROCESS_ERROR_FORK; //Error al ejecutar fork
        }else if(pid_skype == 0){ //child
            if(execlp("skype", "skype", NULL) < 0){
                printf("ProcessManager::ExecSkype: Error in exec: %s\n", strerror(errno));
                exit(1);
            }
        }else{
            return PROCESS_OK;
        }
	}

    int ProcessManager::ExecImageView(char *topic){
        char arg[] = "image:=";
        strcat(arg, topic);

        int status;
        pid_t waitreturnpid;

        if(pid_image_view > 0){	//Check if it's already running
            waitreturnpid = waitpid(pid_image_view, &status, WNOHANG);
            if(waitreturnpid == 0){
                printf("ProcessManager::ExecImageView: process already running\n");
                return PROCESS_RUNNING;	//process already running
            }if(waitreturnpid < 0){
                printf("ProcessManager::ExecImageView: Error in waitpid: %s\n", strerror(errno));
                pid_image_view = -1;
                return PROCESS_ERROR_WAITING;
            }
        }

        if((pid_image_view = fork()) == -1){
            printf("ProcessManager::ExecImageView: Error in fork: %s\n", strerror(errno));
            return PROCESS_ERROR_FORK; //Error al ejecutar fork
        }else if(pid_image_view == 0){ //child
            if(execlp("rosrun", "rosrun", "image_view", "image_view", arg, NULL) < 0){
                printf("ProcessManager::ExecImageView: Error in exec: %s\n", strerror(errno));
                exit(1);
            }

        }else{
            return PROCESS_OK;
        }
    }

	int ProcessManager::ExecRviz(){
        int status;
        pid_t waitreturnpid;

        if(pid_rviz > 0){	//Check if it's already running
            waitreturnpid = waitpid(pid_rviz, &status, WNOHANG);
            if(waitreturnpid == 0){
                printf("ProcessManager::ExecRviz: process already running\n");
                return PROCESS_RUNNING;	//process already running
            }if(waitreturnpid < 0){
                printf("ProcessManager::ExecRviz: Error in waitpid: %s\n", strerror(errno));
                pid_rviz = -1;
                return PROCESS_ERROR_WAITING;
            }
        }

        if((pid_rviz = fork()) == -1){
            printf("ProcessManager::ExecRviz: Error in fork: %s\n", strerror(errno));
            return PROCESS_ERROR_FORK; //Error al ejecutar fork
        }else if(pid_rviz == 0){ //child
            if(execlp("rosrun", "rosrun", "rviz", "rviz", NULL) < 0){
                printf("ProcessManager::ExecRviz: Error in exec: %s\n", strerror(errno));
                exit(1);
            }
        }else{
            return PROCESS_OK;
        }
	}

	int ProcessManager::ExecOpenRAVE(int object_id){
        int status;
        pid_t waitreturnpid;

        if(pid_OpenRAVE > 0){	//Check if it's already running
            waitreturnpid = waitpid(pid_OpenRAVE, &status, WNOHANG);
            if(waitreturnpid == 0){
                printf("ProcessManager::ExecOpenRAVE: process already running\n");
                return PROCESS_RUNNING;	//process already running
            }if(waitreturnpid < 0){
                printf("ProcessManager::ExecOpenRAVE: Error in waitpid: %s\n", strerror(errno));
                pid_OpenRAVE = -1;
                return PROCESS_ERROR_WAITING;
            }
        }

        if((pid_OpenRAVE = fork()) == -1){
            printf("ProcessManager::ExecOpenRAVE: Error in fork: %s\n", strerror(errno));
            return PROCESS_ERROR_FORK; //Error al ejecutar fork
        }else if(pid_OpenRAVE == 0){ //child
            if(execlp("rosrun", "rosrun", "srs_grasping", "simulation.py", object_id, NULL) < 0){
                printf("ProcessManager::ExecOpenRAVE: Error in exec: %s\n", strerror(errno));
                exit(1);
            }
        }else{
            return PROCESS_OK;
        }
    }


/*!	\fn int ProcessManager::ExecProcess(char *program_path, char **args)
 * 	\brief Creates and exec another program
 * 	\return PROCESS_OK
 * 	\return PROCESS_ERROR
*/
int ProcessManager::ExecProcess(char *program_path, char **args){
	return PROCESS_OK;
}


/*!	\fn int ProcessManager::WaitTurtleSim()
 * 	\brief Waits for the termination of TurtleSim
 * 	\return PROCESS_OK
 * 	\return PROCESS_ERROR_WAITING
*/
int ProcessManager::WaitSkype(){
	pid_t waitreturnpid;
	int status;

	if(pid_skype > 0){	//Check if it's already running
		do{
			waitreturnpid = waitpid(pid_skype, &status, WNOHANG);
			if(waitreturnpid == 0){
				printf("ProcessManager::WaitSkype: Running\n");
				return PROCESS_RUNNING;
			}

			if(waitreturnpid < 0){
				printf("ProcessManager::WaitSkype: Error in waitpid: %s\n", strerror(errno));
				return PROCESS_ERROR_WAITING;
			}

			if (WIFEXITED(status)) {
				printf("ProcessManager::WaitSkype:child exited, status= %d\n", WEXITSTATUS(status));

			} else if (WIFSIGNALED(status)) {
				printf("ProcessManager::WaitSkype: child killed (signal %d)\n", WTERMSIG(status));


			} else if (WIFSTOPPED(status)) {
				printf("ProcessManager::WaitSkype: child stopped (signal %d)\n", WSTOPSIG(status));
			}

    	} while (!WIFEXITED(status) && !WIFSIGNALED(status));

		pid_skype = -1;
		return PROCESS_OK;
	}else{
		printf("ProcessManager::WaitSkype: no process to wait for..\n");
		return PROCESS_OK;
	}
}

int ProcessManager::WaitImageView(){
	pid_t waitreturnpid;
	int status;

	if(pid_image_view > 0){	//Check if it's already running
		do{
			waitreturnpid = waitpid(pid_image_view, &status, WNOHANG);
			if(waitreturnpid == 0){
				printf("ProcessManager::WaitImageView: Running\n");
				return PROCESS_RUNNING;
			}

			if(waitreturnpid < 0){
				printf("ProcessManager::WaitImageView: Error in waitpid: %s\n", strerror(errno));
				return PROCESS_ERROR_WAITING;
			}

			if (WIFEXITED(status)) {
				printf("ProcessManager::WaitImageView:child exited, status= %d\n", WEXITSTATUS(status));

			} else if (WIFSIGNALED(status)) {
				printf("ProcessManager::WaitImageView: child killed (signal %d)\n", WTERMSIG(status));


			} else if (WIFSTOPPED(status)) {
				printf("ProcessManager::WaitImageView: child stopped (signal %d)\n", WSTOPSIG(status));
			}

    	} while (!WIFEXITED(status) && !WIFSIGNALED(status));

		pid_image_view = -1;
		return PROCESS_OK;
	}else{
		printf("ProcessManager::WaitImageView: no process to wait for..\n");
		return PROCESS_OK;
	}
}

int ProcessManager::WaitRviz(){
	pid_t waitreturnpid;
	int status;

	if(pid_rviz > 0){	//Check if it's already running
		do{
			waitreturnpid = waitpid(pid_rviz, &status, WNOHANG);
			if(waitreturnpid == 0){
				printf("ProcessManager::WaitRviz: Running\n");
				return PROCESS_RUNNING;
			}

			if(waitreturnpid < 0){
				printf("ProcessManager::WaitRviz: Error in waitpid: %s\n", strerror(errno));
				return PROCESS_ERROR_WAITING;
			}

			if (WIFEXITED(status)) {
				printf("ProcessManager::WaitRviz:child exited, status= %d\n", WEXITSTATUS(status));

			} else if (WIFSIGNALED(status)) {
				printf("ProcessManager::WaitRviz: child killed (signal %d)\n", WTERMSIG(status));


			} else if (WIFSTOPPED(status)) {
				printf("ProcessManager::WaitRviz: child stopped (signal %d)\n", WSTOPSIG(status));
			}

    	} while (!WIFEXITED(status) && !WIFSIGNALED(status));

		pid_rviz = -1;
		return PROCESS_OK;
	}else{
		printf("ProcessManager::WaitRviz: no process to wait for..\n");
		return PROCESS_OK;
	}
}

int ProcessManager::WaitOpenRAVE(){
	pid_t waitreturnpid;
	int status;

	if(pid_OpenRAVE > 0){	//Check if it's already running
		do{
			waitreturnpid = waitpid(pid_OpenRAVE, &status, WNOHANG);
			if(waitreturnpid == 0){
				printf("ProcessManager::WaitOpenRAVE: Running\n");
				return PROCESS_RUNNING;
			}

			if(waitreturnpid < 0){
				printf("ProcessManager::WaitOpenRAVE: Error in waitpid: %s\n", strerror(errno));
				return PROCESS_ERROR_WAITING;
			}

			if (WIFEXITED(status)) {
				printf("ProcessManager::WaitOpenRAVE:child exited, status= %d\n", WEXITSTATUS(status));

			} else if (WIFSIGNALED(status)) {
				printf("ProcessManager::WaitOpenRAVE: child killed (signal %d)\n", WTERMSIG(status));


			} else if (WIFSTOPPED(status)) {
				printf("ProcessManager::WaitOpenRAVE: child stopped (signal %d)\n", WSTOPSIG(status));
			}

    	} while (!WIFEXITED(status) && !WIFSIGNALED(status));

		pid_OpenRAVE = -1;
		return PROCESS_OK;
	}else{
		printf("ProcessManager::WaitOpenRAVE: no process to wait for..\n");
		return PROCESS_OK;
	}
}


/*!	\fn int ProcessManager::ExitTurtleSim()
 * 	\brief Sends a signal to the process TurtleSim to finish the execution
 * 	\return PROCESS_OK
 * 	\return PROCESS_ERRROR_KILL
*/
int ProcessManager::ExitSkype(){
	if(pid_skype > 0){
		if(kill(pid_skype, SIGINT)!=0){
			printf("ProcessManager::ExitSkype: Error sending the signal: %s\n", strerror(errno));
			return PROCESS_ERRROR_KILL;
		}else{
			pid_skype = -1;
			return PROCESS_OK;
		}
	}else{
		printf("ProcessManager::ExitSkype: process is not running\n");
		return PROCESS_OK;
	}
}

int ProcessManager::ExitImageView(){
	if(pid_image_view > 0){
		if(kill(pid_image_view, SIGINT)!=0){
			printf("ProcessManager::ExitImageView: Error sending the signal: %s\n", strerror(errno));
			return PROCESS_ERRROR_KILL;
		}else{
			pid_image_view = -1;
			return PROCESS_OK;
		}
	}else{
		printf("ProcessManager::ExitImageView: process is not running\n");
		return PROCESS_OK;
	}
}

int ProcessManager::ExitRviz(){
	if(pid_rviz > 0){
		if(kill(pid_rviz, SIGINT)!=0){
			printf("ProcessManager::ExitRviz: Error sending the signal: %s\n", strerror(errno));
			return PROCESS_ERRROR_KILL;
		}else{
			pid_rviz = -1;
			return PROCESS_OK;
		}
	}else{
		printf("ProcessManager::ExitRviz: process is not running\n");
		return PROCESS_OK;
	}
}

int ProcessManager::ExitOpenRAVE(){
	if(pid_OpenRAVE > 0){
		if(kill(pid_OpenRAVE, SIGINT)!=0){
			printf("ProcessManager::ExitOpenRAVE: Error sending the signal: %s\n", strerror(errno));
			return PROCESS_ERRROR_KILL;
		}else{
			pid_OpenRAVE = -1;
			return PROCESS_OK;
		}
	}else{
		printf("ProcessManager::ExitOpenRAVE: process is not running\n");
		return PROCESS_OK;
	}
}


/*!	\fn int ProcessManager::KillTurtleSim()
 * 	\brief Kills the process TurtleSim
 * 	\return PROCESS_OK
 * 	\return PROCESS_ERRROR_KILL
*/
int ProcessManager::KillSkype(){
	if(pid_skype > 0){
		if(kill(pid_skype, SIGKILL)!=0){
			printf("ProcessManager::KillSkype: Error sending the signal: %s\n", strerror(errno));
			return PROCESS_ERRROR_KILL;
		}else{
			pid_skype = -1;
			return PROCESS_OK;
		}
	}else{
		printf("ProcessManager::KillSkype: process is not running\n");
		return PROCESS_OK;
	}
}

int ProcessManager::KillImageView(){
	if(pid_image_view > 0){
		if(kill(pid_image_view, SIGKILL)!=0){
			printf("ProcessManager::KillImageView: Error sending the signal: %s\n", strerror(errno));
			return PROCESS_ERRROR_KILL;
		}else{
			pid_image_view = -1;
			return PROCESS_OK;
		}
	}else{
		printf("ProcessManager::KillImageView: process is not running\n");
		return PROCESS_OK;
	}
}

int ProcessManager::KillRviz(){
	if(pid_rviz > 0){
		if(kill(pid_rviz, SIGKILL)!=0){
			printf("ProcessManager::KillRviz: Error sending the signal: %s\n", strerror(errno));
			return PROCESS_ERRROR_KILL;
		}else{
			pid_rviz = -1;
			return PROCESS_OK;
		}
	}else{
		printf("ProcessManager::KillRviz: process is not running\n");
		return PROCESS_OK;
	}
}

int ProcessManager::KillOpenRAVE(){
	if(pid_OpenRAVE > 0){
		if(kill(pid_OpenRAVE, SIGKILL)!=0){
			printf("ProcessManager::KillOpenRAVE: Error sending the signal: %s\n", strerror(errno));
			return PROCESS_ERRROR_KILL;
		}else{
			pid_OpenRAVE = -1;
			return PROCESS_OK;
		}
	}else{
		printf("ProcessManager::KillOpenRAVE: process is not running\n");
		return PROCESS_OK;
	}
}
